
#include <public/globalsensor_tm_store.h>

#define PACK_BYTES_FOR_STORING_TM_FRAMES 253
#define TM_FRAME_HEADER_AND_CRC_SIZE 18
#define TM_FRAME_HEADER_SIZE 16


void CGlobalSensorTMStore::add_free_pack_list(CPackListHandler &packList){

	packList.ExtractAllPackages(m_free_pack_list);

}

void CGlobalSensorTMStore::get_free_pack_list(CPackListHandler &packList){

	m_free_pack_list.ExtractAllPackages(packList);
}

void CGlobalSensorTMStore::get_tm_pack_list(CPackListHandler &packList){

	if(m_currentPack.IsValid()){

		PaddingWithZeroesCurrentPack();

		m_tm_pack_list.AddPack(m_currentPack);
		m_currentPackIndex=0;
		m_currentPackFrameNumber=0;
	}

	m_tm_pack_list.ExtractAllPackages(packList);
}

void CGlobalSensorTMStore::restore_tm_pack_list(CPackListHandler &packList){

	packList.ExtractAllPackages(m_tm_pack_list);

}

bool_t CGlobalSensorTMStore::store_ssi_tm_frame(byte_t *pFrame){


	bool tm_frame_stored=true;


	if(!m_currentPack.IsValid()){

		 if(m_free_pack_list.TakeOnePack(m_currentPack)){
			 m_currentPackIndex=1;
			 m_currentPackFrameNumber=0;
		 }

	}else if (!frameCanBeStoredInPack(pFrame)){

		//Padding with zeroes
		PaddingWithZeroesCurrentPack();

		//store current pack
		m_tm_pack_list.AddPack(m_currentPack);

		 if(m_free_pack_list.TakeOnePack(m_currentPack)){
			 m_currentPackIndex=1; //Puntero dentro del current Pack
			 m_currentPackFrameNumber=0;
		 }
	}

	if(m_currentPack.IsValid()){
		//Guardar frame_info
				//todo

				byte_t aux;
				byte_t aux2;
				uint8_t length; //var for the length

				//
				for(int i=0; i<(TM_FRAME_HEADER_SIZE-1); i++){
					aux=*pFrame;

					m_currentPack.WriteByte(m_currentPackIndex,aux);


				//cout<<aux<<endl;
					m_currentPackIndex++;
					pFrame++;

				}

				length =*pFrame;
				m_currentPack.WriteByte(m_currentPackIndex,length);
				m_currentPackIndex++;
				pFrame++;

               for (int j=0; j<length;j++){
            	   aux2=*pFrame;

            	   m_currentPack.WriteByte(m_currentPackIndex, aux2);
            	   m_currentPackIndex++;
            	   pFrame++;
               }
				//todo


		m_currentPackFrameNumber++;
		m_currentPack.WriteByte(0,m_currentPackFrameNumber);

	} else tm_frame_stored=false;


	return tm_frame_stored;

}


bool CGlobalSensorTMStore::frameCanBeStoredInPack(byte_t *pFrame){

	uint8_t freebytes=PACK_BYTES_FOR_STORING_TM_FRAMES + 1 -m_currentPackIndex;
	uint8_t datalength = ssi_link_layer_get_data_length(pFrame);

	return(freebytes>(TM_FRAME_HEADER_AND_CRC_SIZE+datalength));

}

void CGlobalSensorTMStore::PaddingWithZeroesCurrentPack(){

	if(m_currentPack.IsValid()){

		while( m_currentPackIndex < (PACK_BYTES_FOR_STORING_TM_FRAMES+1)){
				m_currentPack.WriteByte(m_currentPackIndex,0);
				m_currentPackIndex++;

		}
	}
}


